package main
import (
	"log"
	"math"
	"time"
)

func RoutineSvTest() {
	mem, _ := ShmOpen(ShmName, SmpDataCnt*SmpDataLen)
	offset := 0
	var smpData1 []int32 = make([]int32, 80)
	var smpData2 []int32 = make([]int32, 80)
	var smpData3 []int32 = make([]int32, 80)
	for {
		time.Sleep(2 * time.Second)
		for i := 0; i < 80; i++ {
			idx := (offset + i) * SmpDataLen
			smpData1[i] = DecodeInt(mem[idx : idx+4])
			smpData2[i] = DecodeInt(mem[idx+8 : idx+12])
			smpData3[i] = DecodeInt(mem[idx+16 : idx+20])
		}
		calculate(smpData1, smpData2, smpData3)
		offset += 120
		if offset+80 >= SmpDataCnt {
			offset = 0
		}
	}
}

func calculate(smpData1 []int32, smpData2 []int32, smpData3 []int32) {
	N := 80 //len(smpData1)

	var fa1, fa2, fa3 float64 = 0, 0, 0
	var fb1, fb2, fb3 float64 = 0, 0, 0
	for i := 0; i < N; i++ {
		c1 := float64(i) * 2 * math.Pi / float64(N)
		a1 := float64(smpData1[i]) / 10000.0
		fa1 += a1 * math.Sin(c1)
		fb1 += a1 * math.Cos(c1)

		a2 := float64(smpData2[i]) / 10000.0
		fa2 += a2 * math.Sin(c1)
		fb2 += a2 * math.Cos(c1)

		a3 := float64(smpData3[i]) / 10000.0
		fa3 += a3 * math.Sin(c1)
		fb3 += a3 * math.Cos(c1)
	}
	amp1 := 2 * math.Sqrt(fa1*fa1+fb1*fb1) / float64(N)
	pha1 := math.Atan2(-fa1, fb1)
	amp2 := 2 * math.Sqrt(fa2*fa2+fb2*fb2) / float64(N)
	pha2 := math.Atan2(-fa2, fb2)
	amp3 := 2 * math.Sqrt(fa3*fa3+fb3*fb3) / float64(N)
	pha3 := math.Atan2(-fa3, fb3)
	log.Printf("smpData1: (%f, %f), (%f, %f), (%f, %f) ", amp1/math.Sqrt(2), pha1, amp2/math.Sqrt(2), pha2, amp3/math.Sqrt(2), pha3)
	return
}